/** sched.c: Top level implementation of the scheduler.
 *
 * Author: 	Nora Tarano <ntarano@andrew.cmu.edu>
 *          Yuan Meng <yuanm@andrew.cmu.edu>
 *          Yipeng Yun <yipengy@andrew.cmu.edu>
 * Date: 	Dec 11, 2011
 */

#include <types.h>
#include <assert.h>

#include <kernel.h>
#include <config.h>
#include "sched_i.h"

#include <arm/reg.h>
#include <arm/psr.h>
#include <arm/exception.h>
#include <arm/physmem.h>

tcb_t system_tcb[OS_MAX_TASKS]; /* allocate memory for system TCBs */

#define MAX_TASK    64
#define IDLE_TASK   63

#ifndef NULL
#define NULL    ((void *)0x0)
#endif

/*
 * Insertion sort to put all the tasks in rate monotonic order
 * index 0 is highest priority, index 63 is lowest priority
 */

static void sort_rms(task_t** tasks, size_t num_tasks)
{
	int i, j;
	task_t *key;

	for (i = 0; i < (int) num_tasks; i++)
	{
		key = tasks[i];
		for(j = i-1; (j >= 0) && (key->T < tasks[j]->T); j--)
			tasks[j+1] = tasks[j];
		tasks[j+1] = key;
	}
}

void sched_init(task_t* main_task)
{
	allocate_tasks(&main_task, (uint32_t)1);
}

/**
 * @brief This is the idle task that the system runs when no other task is runnable
 */
 
static void idle(void)
{
	 enable_interrupts();
	 while(1);
}

/**
 * @brief Allocate user-stacks and initializes the kernel contexts of the
 * given threads.
 *
 * This function assumes that:
 * - num_tasks < number of tasks allowed on the system.
 * - the tasks have already been deemed schedulable and have been appropriately
 *   scheduled.  In particular, this means that the task list is sorted in order
 *   of priority -- higher priority tasks come first.
 *
 * @param tasks  A list of scheduled task descriptors.
 * @param size   The number of tasks is the list.
 */
void allocate_tasks(task_t** tasks, size_t num_tasks)
{
	unsigned int i;
    
    /* clear the run queue */
    runqueue_init();
    
    // order tasks list in rate monotonic order
    sort_rms(tasks, num_tasks);
    
    /* loop thru all tasks that we need to run, set up tcb for each one */
    for (i = 0; i < num_tasks; i++)
    {
        system_tcb[i].native_prio = i;
        system_tcb[i].cur_prio = i;
        system_tcb[i].sleep_queue = NULL;
        system_tcb[i].holds_lock = 0;
        
        // user entry pt
        system_tcb[i].context.r4 = (uint32_t)tasks[i]->lambda;
		
		// single argument to user function
		system_tcb[i].context.r5 = (uint32_t)tasks[i]->data;
		
		// stack_pos
		system_tcb[i].context.r6 = (uint32_t)tasks[i]->stack_pos;
        
		// r8
		system_tcb[i].context.r8 = (uint32_t)global_data;
        
		// stack
		system_tcb[i].context.sp = system_tcb[i].kstack_high;
		
		// lr
		system_tcb[i].context.lr = launch_task;
        
		// other registers		
		system_tcb[i].context.r7 = 0x0;
		system_tcb[i].context.r9 = 0x0;
		system_tcb[i].context.r10 = 0x0;
		system_tcb[i].context.r11 = 0x0;
        
		runqueue_add(&system_tcb[i], i);
    }
    
    /* add the idle task */
	i = IDLE_TASK;
    
	system_tcb[i].cur_prio = i;
	system_tcb[i].sleep_queue = NULL;
	system_tcb[i].native_prio = i;
	system_tcb[i].holds_lock = 0;
	
	// user entry point
	system_tcb[i].context.r4 = (uint32_t)tasks[i]->lambda;
	
	// single argument to user function
	system_tcb[i].context.r5 = (uint32_t)tasks[i]->data;
	
	// stack_pos
	system_tcb[i].context.r6 = (uint32_t)tasks[i]->stack_pos;
	// r8 
	system_tcb[i].context.r8 = (uint32_t)global_data;
    
	// stack
	system_tcb[i].context.sp = system_tcb[i].kstack_high;
	
	// lr
	system_tcb[i].context.lr = idle;
    
	// other registers	
	system_tcb[i].context.r7 = 0x0;
	system_tcb[i].context.r9 = 0x0;
	system_tcb[i].context.r10 = 0x0;
	system_tcb[i].context.r11 = 0x0;
    
	runqueue_add(&system_tcb[i], i);
    
	// we are on init, so go to the idle task
	dispatch_init(&(system_tcb[IDLE_TASK]));
}

